clear ;
clc ;
load( 'data.mat' ) ;
% Pre-whittening
covMatrix = cov(ts', 1);
[E, D] = eig(covMatrix) ;
V = E * D^(-1/2) * E' ;
Z = V * ts ;
% ICA iterations
iter = 1 ;
W = eye( size( Z, 1 ) ) ;
Z0 = Z ;
while( iter < 100 )
    Y = W * Z0 ;
    Ypos = max( Y, 0 ) ;
    Yneg = Y - Ypos ;
    % Compute torque
    g = Ypos * Yneg' - Yneg * Ypos' ;
    % Find the maximum of absolute of g
    [ maxcolabsg, indices ] = nanmax( abs(g) ) ;
    [ maxabsg, i ] = nanmax( nanmax( abs(g) ) ) ; 
    if maxabsg < 0.001
        break ;
    end
    % Construct the reduced Z
    j = indices( i ) ;
    Zred = Z( [ i j ], : ) ;
    J2 = Yneg( i, : ) * Yneg( i, : )' + Yneg( j, : ) * Yneg( j, : )' ;
    Jgrad = g( i, j ) ;
    JHess = Ypos( i, Ypos(i,:) > 0 & Ypos(j,:) == 0 ) * ...
            Ypos( i, Ypos(i,:) > 0 & Ypos(j,:) == 0 )' + ...
            Ypos( j, Ypos(i,:) == 0 & Ypos(j,:) > 0 ) * ...
            Ypos( j, Ypos(i,:) == 0 & Ypos(j,:) > 0 )' - ...
            Yneg( i, Yneg(i,:) < 0 & Yneg(j,:) == 0 ) * ...
            Yneg( i, Yneg(i,:) < 0 & Yneg(j,:) == 0 )' - ...
            Yneg( j, Yneg(i,:) == 0 & Yneg(j,:) < 0 ) * ...
            Yneg( j, Yneg(i,:) == 0 & Yneg(j,:) < 0 )' ;
    phi = 0 ;
    while( abs( Jgrad ) >= 10 )
        phi = phi + Jgrad / JHess ;
        Wred = [ cos(phi) sin(phi); -sin(phi) cos(phi) ] ;
        Yred = Wred * Zred ;
        Yredpos = max( Yred, 0 ) ;
        Yredneg = Yred - Yredpos ;
        Jgrad = Yredpos( 1, : ) * Yredneg( 2, : )' - Yredneg( 1, : ) * Yredpos( 2, : )' ;
        J2 = Yredneg( 1, : ) * Yredneg( 1, : )' + Yredneg( 2, : ) * Yredneg( 2, : )' ;
        JHess = Yredpos( 1, Yredpos(1,:) > 0 & Yredpos(2,:) == 0 ) * ...
            Yredpos( 1, Yredpos(1,:) > 0 & Yredpos(2,:) == 0 )' + ...
            Yredpos( 2, Yredpos(1,:) == 0 & Yredpos(2,:) > 0 ) * ...
            Yredpos( 2, Yredpos(1,:) == 0 & Yredpos(2,:) > 0 )' - ...
            Yredneg( 1, Yredneg(1,:) < 0 & Yredneg(2,:) == 0 ) * ...
            Yredneg( 1, Yredneg(1,:) < 0 & Yredneg(2,:) == 0 )' - ...
            Yredneg( 2, Yredneg(1,:) == 0 & Yredneg(2,:) < 0 ) * ...
            Yredneg( 2, Yredneg(1,:) == 0 & Yredneg(2,:) < 0 )' ;
    end
    if JHess < 0
        phi = pi ;
        while( abs( Jgrad ) >= 10 )
            phi = phi + Jgrad / JHess ;
            Wred = [ cos(phi) sin(phi); -sin(phi) cos(phi) ] ;
            Yred = Wred * Zred ;
            Yredpos = max( Yred, 0 ) ;
            Yredneg = Yred - Yredpos ;
            Jgrad = Yredpos( 1, : ) * Yredneg( 2, : )' - Yredneg( 1, : ) * Yredpos( 2, : )' ;
            J2 = Yredneg( 1, : ) * Yredneg( 1, : )' + Yredneg( 2, : ) * Yredneg( 2, : )' ;
            JHess = Yredpos( 1, Yredpos(1,:) > 0 & Yredpos(2,:) == 0 ) * ...
                Yredpos( 1, Yredpos(1,:) > 0 & Yredpos(2,:) == 0 )' + ...
                Yredpos( 2, Yredpos(1,:) == 0 & Yredpos(2,:) > 0 ) * ...
                Yredpos( 2, Yredpos(1,:) == 0 & Yredpos(2,:) > 0 )' - ...
                Yredneg( 1, Yredneg(1,:) < 0 & Yredneg(2,:) == 0 ) * ...
                Yredneg( 1, Yredneg(1,:) < 0 & Yredneg(2,:) == 0 )' - ...
                Yredneg( 2, Yredneg(1,:) == 0 & Yredneg(2,:) < 0 ) * ...
                Yredneg( 2, Yredneg(1,:) == 0 & Yredneg(2,:) < 0 )' ;
        end
    end
    R = eye( size( Z, 1 ) ) ;
    R( i, i ) = cos( phi ) ;
    R( j, j ) = cos( phi ) ;
    R( i, j ) = sin( phi ) ;
    R( j, i ) = -sin( phi ) ;
    W = R * W ;
    Z = R * Z ;
    iter = iter + 1 ;
end